#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>

nav_msgs::Odometry odom;
double _rate;

void odom_callback(const nav_msgs::Odometry::ConstPtr &msg)
{
   odom = *msg;
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "odom_to_mavros");

  ros::NodeHandle nh("~");

  ros::Subscriber odom_sub = nh.subscribe<nav_msgs::Odometry> ("odom", 2, odom_callback);

  ros::Publisher pos_pub=nh.advertise<geometry_msgs::PoseStamped> ("mavros",10);

  geometry_msgs::PoseStamped pose;

  nh.param("rate", _rate, 30.0);
  
  ros::Rate rate(_rate);

  ROS_INFO("Publish Mavros vision_pose...");

  while (ros::ok())
  {
    pose.header.stamp = ros::Time::now();
    pose.header.frame_id = "base_link";

    pose.pose.position.x = odom.pose.pose.position.x;
    pose.pose.position.y = odom.pose.pose.position.y;
    pose.pose.position.z = odom.pose.pose.position.z;

    pose.pose.orientation.x = odom.pose.pose.orientation.x;
    pose.pose.orientation.y = odom.pose.pose.orientation.y;
    pose.pose.orientation.z = odom.pose.pose.orientation.z;
    pose.pose.orientation.w = odom.pose.pose.orientation.w;
    
    pos_pub.publish(pose);
    ros::spinOnce();
    rate.sleep();
  }

  return 0;
}